function [longmod,spmod,latmod,linmodel]=linearize_vtp(OperatingPoint,use_uvw,verbose)
% [longmod,spmod,latmod,linmodel]=linearize_vtp(OperatingPoint,verbose)
%
% Linearizes the UAV model about a given operating point using
% ../NL_Sim/UAV_NL.mdl. This function can be called from any of the three
% sim directories. However, this function will use your workspace
% variables. Requires the Control System Toolbox and Simulink Control
% Design.
%
% Inputs:
%   OperatingPoint - Operating point object of a trim condition
%   use_uvw        - boolean flag to use u,v,w as linear model outputs 
%                     instead of V, alpha, beta; defaults to "false"
%   verbose        - boolean flag to suppress output; default "true"
%
% Outputs:
%   longmod  - longitudinal linear model
%   spmod    - short period approximation
%   latmod   - lateral directional linear model
%   linmodel - full linear model
%
%


% verbose output on by default
switch nargin 
    case 1
        verbose = 1;
        use_uvw = 0;
    case 2
        verbose = 1;
end

%% Load model into memory
isLoaded = bdIsLoaded('VTP_airframe'); % check to see if model is loaded or open
if ~isLoaded
    load_system('VTP_airframe.mdl') % load model into system memory without opening diagram
end

%% LINEARIZE
linmodel = linearize('VTP_airframe',OperatingPoint.op_point);

% Create Full 12-state Linear Model
set(linmodel, 'InputName', {'d_a';'d_e'; 'd_r' ;'d_f';'d_T'});
set(linmodel, 'OutputName',{'xe';'ye';'ze';'phi'; 'theta'; 'psi'; ...
    'ub'; 'vb'; 'wb';'p';'q';'r';'phidot';'thetadot';'psidot'; ...
    'alpha';'beta';'gamma';'V'});
linmodel.c(7:9,10:12) = eye(3);

set(linmodel, 'StateName', {'phi';'theta';'psi';'p';'q';'r';'ub';'vb';'wb';...
    'Xe';'Ye';'Ze'});

%% GENERATE LONGITUDINAL LINEAR MODEL
% Longitudinal dynamics
% States: ub(7)  wb(9)  q(5) theta(2) Ze(12)
% Inputs: elevator(2) throttle(5)
% Outputs: V(19) alpha(16) q(11) theta(5) h(3)

% Generate State-space matrices for Longitudinal Model
% Indices for desired state, outputs, and inputs
Xlon = [7 9 5 2 12];
Ylon = [19 16 11 5 3];
Ilon = [2 5];
longmod = modred(linmodel(Ylon,Ilon),setdiff(1:12,Xlon),'Truncate');
longmod = xperm(longmod,[1 2 3 4 5]); % reorder state

if verbose
    fprintf('\n  Longitudinal Model\n------------------------\n');    
    longmod
    fprintf('\n\nLongitudinal Poles:');
    damp(longmod)
end

%% GENERATE SHORT PERIOD LINEAR MODEL
% Short period dynamics
% States:   wb(9)  q(5) 
% Inputs: elevator(2) 
% Outputs:  alpha(16) q(11)

% Generate State-space matrices for Longitudinal Model
% Indices for desired state, outputs, and inputs
Xlon = [9 5];
Ylon = [16 11];
Ilon = [2];
spmod = modred(linmodel(Ylon,Ilon),setdiff(1:12,Xlon),'Truncate');

if verbose
    fprintf('\n  Short Period Model\n------------------------\n');    
    spmod
    fprintf('\n\nShort Period Poles:');
    damp(spmod)
end

%% GENERATE LATERAL-DIRECTIONAL LINEAR MODEL
% Lateral-directional dynamics
% States: vb(8) p(4) r(6) phi(1) psi(3)
% Inputs: aileron(1) rudder(3)
% Outputs: beta(17) p(10) r(12) phi(4) psi(6)

% Generate State-space matrices for lateral-directional Model
% Indices for desired state, outputs, and inputs
Xlat = [8 4 6 1 3];
Ylat = [17 10 12 4 6];
Ilat = [1 3];
latmod = modred(linmodel(Ylat,Ilat),setdiff(1:12,Xlat),'Truncate');
latmod = xperm(latmod,[1 2 3 4 5]); % reorder state

if verbose
    fprintf('\n\n  Lateral-Directional Model\n-----------------------------\n');    
    latmod
    fprintf('\n\nLateral-Directional Poles:');
    damp(latmod)
end

% %% Cleanup
if ~isLoaded
     bdclose VTP_airframe % clear model from system memory if we had to load it
end
